#include "lowpass_filter.h"
#include "esp_timer.h"  // for esp_timer_get_time()

void LowPassFilter_Init(LowPassFilter *filter, float Tf) {
    filter->Tf = Tf;
    filter->y_prev = 0.0f;
    filter->timestamp_prev = esp_timer_get_time(); // 单位微秒
}

float LowPassFilter_Update(LowPassFilter *filter, float x) {
    int64_t timestamp = esp_timer_get_time();
    float dt = (timestamp - filter->timestamp_prev) * 1e-6f; // 转换为秒

    if (dt < 0.0f) {
        dt = 1e-3f; // 避免负值
    } else if (dt > 0.3f) {
        filter->y_prev = x;
        filter->timestamp_prev = timestamp;
        ESP_LOGE("lowpass_filter","超时保留");
        return x;
    }

    float alpha = filter->Tf / (filter->Tf + dt);
    float y = alpha * filter->y_prev + (1.0f - alpha) * x;

    filter->y_prev = y;
    filter->timestamp_prev = timestamp;
    return y;
}
